// include 
//#include <webots_config/webots_ros_controller.h>

// int controllerCount = 0;
// std::vector<std_msgs::String> controllerList;
// std::string
// /*============================================== Callback ================================================*/
//     void controllerNameCallback(const std_msgs::String::ConstPtr &name) 
//     { 
//         controllerCount++; 
//         controllerList.push_back(name->data);
//         //将控制器名加入到列表中 
//         ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
// }
/*============================================== Client =============================================*/
/*============================================== Subscriber =============================================*/

#include <webots_ros/Int32Stamped.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/robot_get_device_list.h>

#include <mrobot_tra_controller.h>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
// #include <ros/duration.h>

#define TIME_STEP 32

static int controllerCount;
static std::string controllerName;
static double lposition = 0;
static double rposition = 0;
std::string robot_name = "myMrobot";

/**
 * @brief 构造函数，初始化了ROS的节点功能
 */
TrajectoryController::TrajectoryController()
{
    ros::NodeHandle n;
    // 发布者初始化
    Trajectory_Puber = n.advertise<trajectory_msgs::JointTrajectory>("mrobot_controller/trajectory",10);
    JointState_Puber = n.advertise<sensor_msgs::JointState>("mrobot_controller/joint_states", 10);
    // 关节状态初始化
    JointState_Init(JointStateToPub);
    // 订阅控制目标的信息
    XrobotTarget_Suber = n.subscribe("mrobot_target", 10, &TrajectoryController::MrobotTargetCtrl_Callback, this);
}

int main(int argc, char **argv) 
{
    // 节点初始化
    ros::init(argc, argv, "mrobot_tra_controller");
    ros::NodeHandle n;

    ros::AsyncSpinner spinner(1);
	spinner.start();
    ros::spinOnce();

    // 定义机器人对象
    TrajectoryController Controller;
    // 控制器先初始化
    Controller.Mrobot_Init();
    // 主控制器
    Controller.Mrobot_TraControl();
    
    return 0;
}

// 控制器初始化函数
void TrajectoryController::Mrobot_Init()
{
    static int initCnt = 0;
    ros::Rate initLooprate(1);
    while(!Is_MrobotInit)
    {
        //指定起始点
        SetXrobotStart(l4, 0, l1+l2+l3-10, 0, 0 ,0);
        // 逆运动学解算，移动到轨迹起始点
        myJointState = Mrobot_IK(startPose);
        // 更新要发送的关节信息
        JointState_Update(JointStateToPub,myJointState);
        // 发送关节信息
        PublishJointState(JointStateToPub);
        // 延时等待
        initLooprate.sleep();
        // 计数值自增
        if(++initCnt>5)
        {
            Is_MrobotInit = 1;
            // 初始化结束后，开始时，目标点先暂定为起点
            targetPose.x = startPose.x;
            targetPose.y = startPose.y;
            targetPose.z = startPose.z;
            targetPose.pitch = startPose.pitch;
            targetPose.roll = startPose.roll;
        }
        
    }
    // Is_MrobotInit = 1;
}

std::vector<double> myJointState_watch;
// xrobot机械臂的主控制函数
void TrajectoryController::Mrobot_TraControl()
{
    //初始化完成后，才会进行以下操作
    if(Is_MrobotInit)
    {
        ros::NodeHandle n;
        // 开启单线程 
        ros::AsyncSpinner spinner(1);
        spinner.start();       
        // 设置循环的频率，0.05s一次
        ros::Rate loop_rate(20);
        
        int runningCnt = 0;

        while(ros::ok())
        {
            // 查看中断函数
            ros::spinOnce();
            if(AbleToIK)
            {
                //记录上一次的角度
                for(int i = 0; i <6; i++)
                {
                    lastJointAngle[i] = myJointState[i];
                }
                //计算新的关节角度
                myJointState = Mrobot_IK(targetPose);
                //计算三次多项式的参数
                for(int i=0; i<6; i++)
                {
                    CubicTrajectory(i,lastJointAngle[i],myJointState[i],5);
                }
                runningCnt = 1;
            }
            if(runningCnt<=100)   //计数100下，5s到达
            {
                double runningTime = 0.05*runningCnt;
                for(int i = 0; i <6; i++)
                {
                    myJointState[i] = CubicParam[i][0] + CubicParam[i][1]*runningTime 
                                    + CubicParam[i][2]*runningTime*runningTime + CubicParam[i][3]*runningTime*runningTime*runningTime;
                }
                // 更新要发送的关节信息
                JointState_Update(JointStateToPub,myJointState);
                // 发送关节信息
                PublishJointState(JointStateToPub);
                runningCnt++;
            }

            // 延时等待
            loop_rate.sleep();
        }

        // 关闭线程 
        ros::shutdown();     
    }
}

// 关节数据初始化
void TrajectoryController::JointState_Init(sensor_msgs::JointState &msg)
{
    msg.name.resize(JointNum);
    msg.position.resize(JointNum);   // 分配内存，不然会报错
    msg.header.stamp = ros::Time::now();

    for(int i = 0; i < JointNum; i++) 
    {
        msg.name[i] = JointName[i];
        msg.position[i] = 0;
    }
}

/**
 * @brief 更新关节信息
 * @param msg是要被更新的信息  js是用来更新的信息
 */
void TrajectoryController::JointState_Update(sensor_msgs::JointState &msg, std::vector<double> js)
{
    // 更新关节
    for(int i = 0; i < JointNum; i++) 
    {
        msg.position[i] = js[i];
    }
}


void TrajectoryController::PublishJointState(sensor_msgs::JointState& msg)
{
    JointState_Puber.publish(msg);
}

bool TrajectoryController::Is_InRange(double x,double min,double max)
{
    if(x>=min && x<=max) return true;
    else return false;
}

/*
@brief  mrobot机械臂的三次多项式轨迹规划
@note  输入轨迹的起点、终点、运动持续时间，返回三次多项式的四个参数
*/
void TrajectoryController::CubicTrajectory(int num,double startAngle,double endAngle,double tf)
{
    if(num>=0 && num<=5)
    {
        CubicParam[num][0] = startAngle;
        CubicParam[num][1] = 0;
        CubicParam[num][2] = 3*(endAngle-startAngle)/Pow(tf,2);
        CubicParam[num][3] = -2*(endAngle-startAngle)/Pow(tf,3);
    }
}

/*
@brief  xrobot机械臂的逆运动学解算
@note  输入目标点的xyz和pitch roll，返回六个关节角度
*/
std::vector<double> TrajectoryController::Mrobot_IK(double x, double y, double z, double yaw, double pitch, double roll)
{
    std::vector<double> js_;
    //js_是那六个关节角
    // 1.几何解法
    double a[5];    //中间变量
    double theta[2][6];     //关节角（两种可能性）

    //Joint1
    theta[0][0] = atan2(y,x);
    theta[1][0] = theta[0][0];
    //Joint3
    a[0] = (Pow(x,2)+Pow(y,2)+Pow(z-l1,2)-Pow(l2,2)-Pow(l3,2)-Pow(l4,2))/(2*l2);    //中间变量K
    theta[0][2] = atan2(a[0],sqrt(Pow(l3,2)+Pow(l4,2)-Pow(a[0],2))) + atan2(l3,l4);
    theta[1][2] = -atan2(a[0],sqrt(Pow(l3,2)+Pow(l4,2)-Pow(a[0],2))) + atan2(l3,l4);
    
    for(int i=0;i<2;i++)
    {
        //Joint2
        a[1] = l2 + l3*cos(theta[i][2]) - l4*sin(theta[i][2]);
        a[2] = l3*sin(theta[i][2]) + l4*cos(theta[i][2]);
        a[3] = z-l1;
        // a[4] = x/cos(theta[i][0]);
        a[4] = sqrt(Pow(x,2)+Pow(y,2)); //a[1]~a[4]对应中间变量abcd
        theta[i][1] = atan2(a[1]*a[4]-a[2]*a[3] , a[1]*a[3]+a[2]*a[4]);
        //Joint2&3取负
        theta[i][1] = theta[i][1];
        theta[i][2] = theta[i][2];
        //Joint5
        theta[i][4] = -theta[i][1]-theta[i][2]-pitch;
        //Joint4&6
        theta[i][3] = 0;
        theta[i][5] = -roll;

        //打印几个中间变量来看看
        for(int j=0;j<5;j++)
        {
            cout<<"a["<<j<<"] = "<<a[j]<<endl;
        }
        cout<<endl;
    }
    //存关节角用于发布
    for(int j=0;j<6;j++)
    {
        //打印关节角出来看看
        cout<<"1_joint"<<j+1<<" = "<<theta[0][j]<<endl;
    }
    cout<<endl;
    for(int j=0;j<6;j++)
    {
        //打印出来看看
        cout<<"2_joint"<<j+1<<" = "<<theta[1][j]<<endl;
    }
    
    for(int j=0;j<6;j++)
    {
        js_.push_back(theta[1][j]);
    }
            
    //大多数情况都是第二组动的更少，直接发第二组得了
    //1.首先，六个关节角都要在范围内
    // if(Is_InRange(theta[0][0],-1.57,1.57)&& Is_InRange(theta[0][1],-1.57,1.57)&& Is_InRange(theta[0][2],-1.22,1.57)&& Is_InRange(theta[0][4],-1.57,1.57))
    // {
    //     //如果第二组的六个也在范围内，那就再做后续比较
    //     if(Is_InRange(theta[1][0],-1.58,1.58)&& Is_InRange(theta[1][1],-1.58,1.58)&& Is_InRange(theta[1][2],-1.23,1.58)&& Is_InRange(theta[1][4],-1.58,1.58))
    //     {
    //         //1.1 首先比较joint2,把joint2小的那组发出去
    //         if(abs(theta[0][1]) > abs(theta[1][1]))
    //         {
    //             for(int j=0;j<6;j++)
    //             {
    //                 js_.push_back(theta[1][j]);
    //             }
    //         }
    //         if(abs(theta[0][1]) > abs(theta[1][1]))
    //         {
    //             for(int j=0;j<6;j++)
    //             {
    //                 js_.push_back(theta[1][j]);
    //             }
    //         }
    //     }
    //     //如果第二组的六个不在范围内，直接发第一组
    //     else
    //     {
    //         for(int j=0;j<6;j++)
    //         {
    //             js_.push_back(theta[0][j]);
    //         }
    //     }
    // }
    // //2.如果第一组的六个关节角不在范围内，那就第二组的
    // else if(Is_InRange(theta[1][0],-1.57,1.57)&& Is_InRange(theta[1][1],-1.57,1.57)&& Is_InRange(theta[1][2],-1.22,1.57)&& Is_InRange(theta[1][4],-1.57,1.57))
    // {
    //     for(int j=0;j<6;j++)
    //     {
    //         js_.push_back(theta[1][j]);
    //     }
    // }

        
    

    AbleToIK = 0;

    return js_;

    // 2.仿照课本，算旋转矩阵的解法
    // double a[7];    //中间变量
    // double k[2][3]; //中间变量
    // double theta[4][6];    //解出的关节角(4组)
    // double theta23[4];     //角2+3 

    // //1.计算旋转矩阵R_06
    // R_06[0][0] = cos(yaw)*cos(pitch);
    // R_06[0][1] = cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);
    // R_06[0][2] = cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);
    // R_06[1][0] = sin(yaw)*cos(pitch);
    // R_06[1][1] = sin(yaw)*sin(pitch)*sin(roll)+cos(yaw)*cos(roll);
    // R_06[1][2] = sin(yaw)*sin(pitch)*cos(roll)-cos(yaw)*sin(roll);
    // R_06[2][0] = -sin(pitch);
    // R_06[2][1] = cos(pitch)*sin(roll);
    // R_06[2][2] = cos(pitch)*cos(roll);

    // //Joint1
    // theta[0][0] = atan2(l1,sqrt(Pow(x,2)+Pow(y,2)-Pow(l1,2))) + atan2(y,x);
    // theta[1][0] = theta[0][0];
    // theta[2][0] = atan2(l1,-sqrt(Pow(x,2)+Pow(y,2)-Pow(l1,2))) + atan2(y,x);
    // theta[3][0] = theta[2][0];
    // //Joint3
    // a[0] = (Pow(x,2)+Pow(y,2)+Pow(z,2)-Pow(l1,2)-Pow(l2,2)-Pow(l3,2)-Pow(l4,2))/(2*l2);
    // theta[0][2] = atan2(a[0],sqrt(Pow(l3,2)+Pow(l4,2)-Pow(a[0],2))) - atan2(l3,l4);
    // theta[1][2] = atan2(a[0],-sqrt(Pow(l3,2)+Pow(l4,2)-Pow(a[0],2))) - atan2(l3,l4);
    // theta[2][2] = theta[0][2];
    // theta[3][2] = theta[1][2];
    // //之后求四组关节角
    // for(int i=0;i<4;i++)
    // {
    //     //Joint2
    //     a[1] = cos(theta[i][0])*x + sin(theta[i][0])*y;
    //     a[2] = z;
    //     a[3] = l3 + l2*cos(theta[i][2]);
    //     a[4] = l4 + l2*sin(theta[i][2]);
    //     theta23[i] = atan2(a[2]*a[3]+a[1]*a[4],a[1]*a[3]-a[2]*a[4]);
    //     theta[i][1] = theta23[i] - theta[i][2];
    //     //判断Joint5是否为0
    //     a[5] = sin(theta[i][0])*R_06[0][2] - cos(theta[i][0])*R_06[1][2];
    //     a[6] = cos(theta[i][0])*cos(theta23[i])*R_06[0][2] + sin(theta[i][0])*cos(theta23[i])*R_06[1][2] + sin(theta23[i])*R_06[2][2];
    //     if(a[5]==0 && a[6]==0)
    //     {
    //         theta[i][3] = 0;
    //         theta[i][4] = 0;
    //         theta[i][5] = 0;
    //     }
    //     else
    //     {
    //         //Joint4
    //         theta[i][3] = atan2(a[5],a[6]);
    //         //求完Joint4后求中间矩阵的元素
    //         k[0][0] = cos(theta[i][0])*cos(theta23[i])*cos(theta[i][3]) + sin(theta[i][0])*sin(theta[i][3]);
    //         k[0][1] = sin(theta[i][0])*cos(theta23[i])*cos(theta[i][3]) - cos(theta[i][0])*sin(theta[i][3]);
    //         k[0][2] = sin(theta23[i])*cos(theta[i][3]);
    //         k[1][0] = cos(theta[i][0])*sin(theta23[i]);
    //         k[1][1] = sin(theta[i][0])*cos(theta23[i]);
    //         k[1][2] = -cos(theta23[i]);
    //         //Joint5
    //         theta[i][4] = -atan2(k[0][0]*R_06[0][2]+k[0][1]*R_06[1][2]+k[0][2]*R_06[2][2], k[2][0]*R_06[0][2]+k[2][1]*R_06[1][2]+k[2][2]*R_06[2][2]);
    //         theta[i][5] = atan2(k[2][0]*R_06[0][0]+k[2][1]*R_06[1][0]+k[2][2]*R_06[2][0], k[0][0]*R_06[0][0]+k[0][1]*R_06[1][0]+k[0][2]*R_06[2][0]);
    //     }
    // }
   
}

std::vector<double> TrajectoryController::Mrobot_IK(robotPose_typedef target)
{ 
    return Mrobot_IK(target.x, target.y, target.z, target.yaw, target.pitch, target.roll);
}

// 设置机械臂起始点
void TrajectoryController::SetXrobotStart(double x, double y, double z, double yaw, double pitch, double roll)
{
    startPose.x = x;
    startPose.y = y;
    startPose.z = z;
    startPose.yaw = yaw;
    startPose.pitch = pitch;
    startPose.roll = roll;
}

// 设置机械臂的目标点
void TrajectoryController::SetXrobotTarget(double x, double y, double z, double yaw, double pitch, double roll)
{
    // 首先，保存上一次的目标点
    last_targetPose.x = targetPose.x;
    last_targetPose.y = targetPose.y;
    last_targetPose.z = targetPose.z;
    last_targetPose.pitch = targetPose.pitch;
    last_targetPose.yaw = targetPose.yaw;
    last_targetPose.roll = targetPose.roll;

    // 然后，更新新的目标点
    targetPose.x = x;
    targetPose.y = y;
    targetPose.z = z;
    targetPose.yaw = yaw;
    targetPose.pitch = pitch;
    targetPose.roll = roll;
}

/*
@brief  计算贝塞尔曲线（整条轨迹）
@param  points-贝塞尔控制点(包括起点和终点)  waypoints_num-路径上的点数(不包括起点终点)
@note  输入目标点的xyz和pitch roll，返回六个关节角度
*/
std::vector<trajectory_msgs::JointTrajectoryPoint> 
TrajectoryController::GetBezierTrajectory(std::vector<robotPose_typedef> BezierCtrlPoints, int waypoints_num)
{
    // std::vector<trajectory_msgs::JointTrajectoryPoint> waypoints;   //用于存储轨迹上的全部点的关节信息
    // trajectory_msgs::JointTrajectoryPoint waypoint;  //路径上的点
    // double waypos[3];   //用于存储路径上的xyz坐标
    // int BezierSize = BezierCtrlPoints.size()-1;   //贝塞尔曲线的阶数(如果3个控制点就是2阶)

    // std::vector<double> waypoint_jointAngle;   //用于存储过程中各个位置应到达的关节角
    // double dt = 1.0f/waypoints_num;  //时间间隔
    // static double t;    //bezier曲线上的t

    // double deltaPitch = targetPose.pitch - startPose.pitch; //角度差
    // double deltaRoll = targetPose.roll - startPose.roll;

    // //在给定控制点后进行路径规划
    // for(int i=0;i<waypoints_num;i++)
    // {
    //     t = dt*(i+1); 
    //     //依次计算路径中间点的xyz，根据阶数而定
    //     for(int k=0;k<BezierSize+1;k++)
    //     {
    //         waypos[0] +=  CalC(BezierSize,k) * BezierCtrlPoints[k].x * Pow((1-t),(BezierSize-k)) * Pow(t,k);
    //         waypos[1] +=  CalC(BezierSize,k) * BezierCtrlPoints[k].y * Pow((1-t),(BezierSize-k)) * Pow(t,k);
    //         waypos[2] +=  CalC(BezierSize,k) * BezierCtrlPoints[k].z * Pow((1-t),(BezierSize-k)) * Pow(t,k);
    //     }
    //     // //逆运动学解算
    //     // waypoint_jointAngle = Xrobot_IK(waypos[0], waypos[1], waypos[2], startPose.pitch+t*deltaPitch, startPose.roll+t*deltaRoll);
    //     //将解算出来的关节角存到waypoint中
    //     waypoint.positions.push_back(waypoint_jointAngle[0]);
    //     waypoint.positions.push_back(waypoint_jointAngle[1]);
    //     waypoint.positions.push_back(waypoint_jointAngle[2]);
    //     waypoint.positions.push_back(waypoint_jointAngle[3]);
    //     waypoint.positions.push_back(waypoint_jointAngle[4]);
    //     //存每个点之间的间隔时间
    //     waypoint.time_from_start = ros::Duration(1.0f/Xrobot_Vel*t);
    //     //将waypoint存到轨迹点集中
    //     waypoints.push_back(waypoint);
    //     //清空过程点waypoint的信息
    //     waypoint.positions.clear();
    //     //重置过程点waypoint
    //     waypos[0] = 0;
    //     waypos[1] = 0;
    //     waypos[2] = 0;
    // }
    // return waypoints;
}

/*
@brief  计算贝塞尔曲线上某一点对应的关节角
@param  points-贝塞尔控制点(包括起点和终点)  waypoints_num-路径上的点数(不包括起点终点)  cnt当前是第几点
@note  输入目标点的xyz和pitch roll，返回六个关节角度
*/
std::vector<double> 
TrajectoryController::GetBezierTraAngle(std::vector<robotPose_typedef> BezierCtrlPoints, int waypoints_num, int cnt)
{
    // std::vector<double> waypoint_jointAngle;   //用于存储过程中各个位置应到达的关节角

    // double waypos[3] = {0,0,0};   //用于存储路径上的xyz坐标和pitch,roll角
    // int BezierSize = BezierCtrlPoints.size()-1;   //贝塞尔曲线的阶数(如果3个控制点就是2阶)
    // // int BezierSize = 2;

    // double dt = 1.0f/waypoints_num;  //时间间隔
    // double t = 0;    //bezier曲线上的t

    // double deltaPitch = targetPose.pitch - startPose.pitch; //角度差
    // double deltaRoll = targetPose.roll - startPose.roll;

    // //在给定控制点后进行路径规划
    // t = dt*cnt;  //t的值
    // //依次计算路径中间点的xyz，根据阶数而定
    // for(int k=0;k<BezierSize+1;k++)
    // {
    //     waypos[0] +=  CalC(BezierSize,k) * BezierCtrlPoints[k].x * Pow((1-t),(BezierSize-k)) * Pow(t,k);
    //     waypos[1] +=  CalC(BezierSize,k) * BezierCtrlPoints[k].y * Pow((1-t),(BezierSize-k)) * Pow(t,k);
    //     waypos[2] +=  CalC(BezierSize,k) * BezierCtrlPoints[k].z * Pow((1-t),(BezierSize-k)) * Pow(t,k);
    // }
    // //逆运动学解算
    // waypoint_jointAngle = Xrobot_IK(waypos[0], waypos[1], waypos[2], startPose.pitch+t*deltaPitch, startPose.roll+t*deltaRoll);
    
    // // cout<<"waypos[0]="<<waypos[0]<<" waypos[1]="<<waypos[1]<<" waypos[2]="<<waypos[2]<<endl;
    // // cout<<"waypoint_jointAngle[0]="<<waypoint_jointAngle[0]<<" waypoint_jointAngle[1]="<<waypoint_jointAngle[1]
    // // <<" waypoint_jointAngle[2]="<<waypoint_jointAngle[2]<<" waypoint_jointAngle[3]="<<waypoint_jointAngle[3]<<" waypoint_jointAngle[4]="<<waypoint_jointAngle[4]<<endl;
    // // cout<<endl;
    // return waypoint_jointAngle;
}

// 设置贝塞尔控制点
void TrajectoryController::SetBezierCtrlPoints()
{
    // 设置贝塞尔轨迹点数
    BezierPoints_Num = 30;

    //采用二阶贝塞尔曲线即可
    robotPose_typedef wayPose;
    wayPose.x = (startPose.x+targetPose.x)*0.4 + 15;
    wayPose.y = (startPose.y+targetPose.y)*0.5 + 12;
    wayPose.z = (startPose.z+targetPose.z)*0.6 - 7;
 
    // 先清空
    BezierPoints.clear();
    // 存过程点
    BezierPoints.push_back(startPose);
    // BezierPoints.push_back(wayPose);
    BezierPoints.push_back(targetPose);
}

// 判断目标点是否有改变
bool TrajectoryController::Is_TargetChange()
{
    if(last_targetPose == targetPose)
        return 0;
    else 
        return 1;
}


//***************************************** 回调函数 *******************************************
void TrajectoryController::MrobotTargetCtrl_Callback(mrobot_control::robotPose msg)
{
    if(Is_MrobotInit)
    {
        //更新目标点
        SetXrobotTarget(msg.x, msg.y, msg.z, msg.yaw, msg.pitch, msg.roll);
        //如果目标点已经发送改变，就重新设置起点
        if(Is_TargetChange())
        {
            // 重设起点
            SetXrobotStart(last_targetPose.x, last_targetPose.y, last_targetPose.z, last_targetPose.yaw, last_targetPose.pitch, last_targetPose.roll);
            // 计数值也要清零
            Control_Cnt = 0;
            // 允许做一次逆运动学解算
            AbleToIK = 1;

        } 
    }

    // // 如果目标改变了，重新逆运动学解算
    // myJointState = Xrobot_IK(targetPose);
}

//***************************************** 数学计算相关 ******************************************
/*
@brief  计算阶乘
*/
int TrajectoryController::CalFactorial(int n)
{
    int a = 1;
    if(n>0)
    {
        for(int i=1;i<n+1;i++)
        {
            a *= i;
        }
    }
    return a;
}

/*
@brief  计算组合数
*/
int TrajectoryController::CalC(int n,int m)
{
    int a = 1;
    if(n>=m)
    {
        a = CalFactorial(n)/(CalFactorial(m)*CalFactorial(n-m));
    }
    return a;
}

/*
@brief  计算x的n次方
*/
double TrajectoryController::Pow(double x,int n)
{
    double a = 1.0;
    if(n>0)
    {
        for(int i=0;i<n;i++)
        {
            a *= x;
        }
    }   
    return a;
}


